#ifndef __COMMAND_H__
#include "Command.h"
#endif //__COMMAND_H__

#ifndef __C3DFILEINFO_H__
#include "C3dFileInfo.h"
#endif	//__C3DFILEINFO_H__

#ifndef __ARTICULATEDBODY_H__
#include "ArticulatedBody.h"
#endif	//__ARTICULATEDBODY_H__

#ifndef RealTimeIKui_h
#include "RealTimeIKui.h"
#endif //RealTimeIKui_h

#ifndef __PHYLTERFLGLWINDOW_H__
#include "PhylterGLWindow.h"
#endif	//__PHYLTERFLGLWINDOW_H__

#ifndef	__PHOWARDDATA_H__
#include "PhowardData.h"
#endif


int readSkelFile( FILE* file, ArticulatedBody* skel );

extern RealTimeIKUI *UI;

void LoadModel(void *v)
{
  char *params = (char*)v;
  if(!params){
    params = (char*)fl_file_chooser("Open File?", "{*.skel}", "../src/skels" );
  }

  if(!params)
    return;

  FILE *file = fopen(params, "r");
  //  FILE *file = fopen("../src/skels/leg.skel", "r");
    
  if(file == NULL){
    cout << "Skel file does not exist" << endl;
    return;
  }

  ArticulatedBody *mod = new ArticulatedBody();
  UI->mData->mModels.push_back(mod);
  UI->mData->mSelectedModel = mod;

  readSkelFile(file, mod);
  UI->CreateDofSliderWindow();

  mod->InitModel();
  UI->mGLWindow->mShowModel = true;
  UI->mShowModel_but->value(1);
  UI->mGLWindow->refresh();
  
  cout << "model dof count " << UI->mData->mModels[0]->GetDofCount() << endl;
}

void Solve(void *v)
{
	int numFrames = UI->mData->mSelectedModel->mOpenedC3dFile->GetFrameCount();
	
	int numDofs = UI->mData->mSelectedModel->GetDofCount();
	int numMarkers = UI->mData->mSelectedModel->GetHandleCount();
	int numLimbs = UI->mData->mSelectedModel->GetNodeCount();

	Matd frames = Matd(numFrames, numDofs);

	for(int frameIndex = 0; frameIndex < numFrames; frameIndex++)
	{
		if(numFrames > 1 && frameIndex == 0)
		{
			//lol, skip the first frame of the whole body, it's got missing markers
			frameIndex = 1;
		}

		double epsilon = 0.001;
		bool CBig = false;

		do
		{
			CBig = false;
			Vecd q = Vecd(numDofs);
			Vecd C = Vecd(numMarkers*3);
			Matd J = Matd(numMarkers*3, numDofs, vl_0);

			Model *model = UI->mData->mSelectedModel;

			model->mDofList.GetDofs(&frames[frameIndex]);

			for(int i = 0; i < numMarkers*3; i+=3)
			{
				Vec3d markerPos = UI->mData->mSelectedModel->mOpenedC3dFile->GetMarkerPos(frameIndex, i/3);

				Vec3d c = (markerPos - UI->mData->mSelectedModel->mHandles[i/3]->mGlobalPos);
				C[i*3+0] = c[0];
				C[i*3+1] = c[1];
				C[i*3+2] = c[2];

				if(abs(c[0]) > epsilon || abs(c[1]) > epsilon || abs(c[2]) > epsilon)
				{
					CBig = true;
				}
			}

			/*Find Jacobian here*/

			//not the most efficient way to do it, 
			//but defs more organized and easier to debug if we only go for one handle at a time
			for(int markerIndex = 0; markerIndex < numMarkers; markerIndex++)
			{
				for(int dofIndex = 0; dofIndex < numDofs; dofIndex++)
				{
					int limbIndex = numLimbs-1;
					SetJacobianEntry(markerIndex, dofIndex, model->mLimbs[limbIndex], Mat4d(vl_I), J); 
				}
			}

			/*Debug Matrix*
			double x[123][35];

			for(int markerIndex = 0; markerIndex < numMarkers; markerIndex++)
			{
				for(int dof = 0; dof < numDofs; dof++)
				{
					x[(markerIndex*3)+0][dof] = J[(markerIndex*3)+0][dof];
					x[(markerIndex*3)+1][dof] = J[(markerIndex*3)+1][dof];
					x[(markerIndex*3)+2][dof] = J[(markerIndex*3)+2][dof];
				}
			}
			/**/

			Matd transpose = trans(J);
			Matd JJt = J*transpose;
			Matd JJtInv = inv(JJt);
			Matd PseudoInv = transpose*JJtInv;

			frames[frameIndex] = frames[frameIndex] + (PseudoInv*C);


			UI->mData->mSelectedModel->SetDofs(frames, frameIndex);


		}while(CBig);
	}
}

void Exit(void *v)
{
  exit(0);
}

void LoadC3d(void *v)
{
  if(!UI->mData->mSelectedModel){
    cout << "Load skeleton first";
    return;
  }
  char *params = (char*)v;
  if(!params){
    params = fl_file_chooser("Open File?", "{*.c3d}", "Mocap/" );
  }

  if(!params)
    return;
  
  char *c3dFilename = new char[80];
  
  // load single c3d file
 
  C3dFileInfo *openFile = new C3dFileInfo(params);
  openFile->LoadFile();
  UI->mData->mSelectedModel->mOpenedC3dFile = openFile;
  cout << openFile->GetFrameCount() << endl;

  UI->InitControlPanel();
  UI->mGLWindow->mShowConstraints = true;
  UI->mShowConstr_but->value(1);
}

void SetJacobianEntry(int handleId, int dofId, TransformNode *root, Mat4d parentTransform, Matd &Jacobian)
{
	/*start with the product of all parent transforms*/
	Mat4d currentTransform = parentTransform;

	/*Apply transforms associated with this node
	 * If this node contains the Dof, apply the derivative of that tranform*/
	for(int transformIndex = 0; transformIndex < root->mTransforms.size(); transformIndex++)
	{	
		if(root->mTransforms[transformIndex]->IsDof())
		{
			bool containsTHEDof = false;
			for(int dof = 0; dof < root->mTransforms[transformIndex]->GetDofCount(); dof++)
			{
				if(root->mTransforms[transformIndex]->GetDof(dof)->mId == dofId)
				{
					Mat4d derivativeTransform = root->mTransforms[transformIndex]->GetDerivative(0,dof);
					currentTransform = currentTransform * derivativeTransform;
					containsTHEDof = true;
				}
			}
			if(!containsTHEDof)
			{
				currentTransform = currentTransform * root->mTransforms[transformIndex]->GetTransform(0);
			}
		}
		else
		{
			currentTransform = currentTransform * root->mTransforms[transformIndex]->GetTransform(0);
		}
	}

	/*If this node holds the marker we are evaluating at,
	 * Multiply by that offset and throw the result into the Jacobian*/
	for(int markerIndex = 0; markerIndex < root->mMarkerNodes.size(); markerIndex++)
	{
		if(root->mMarkerNodes[markerIndex]->mNodeIndex == handleId)
		{
			Vec4d hk = Vec4d(root->mMarkerNodes[markerIndex]->mOffset, 1);
			Vecd JacobianEntry = currentTransform * hk;

			double x[3];
			x[0] = JacobianEntry[0];
			x[1] = JacobianEntry[1];
			x[2] = JacobianEntry[2];

			Jacobian[(handleId*3)+0][dofId] = JacobianEntry[0];
			Jacobian[(handleId*3)+1][dofId] = JacobianEntry[1];
			Jacobian[(handleId*3)+2][dofId] = JacobianEntry[2];

			return;
		}
	}

	/*Otherwise, continue on to the children*/
	for(int childIndex = 0; childIndex < root->mChildren.size(); childIndex++)
	{
		SetJacobianEntry(handleId, dofId, ((TransformNode*)root->mChildren[childIndex]), currentTransform, Jacobian);
	}
}
